/*
同步读指令，回读ID1与ID2两个舵机的位置与速度信息
*/
#ifdef U8X8_HAVE_HW_SPI
#include <SPI.h>
#endif
#ifdef U8X8_HAVE_HW_I2C
#include <Wire.h>
#endif
#include <SCServo.h>

SMS_STS sms_sts;

void setup()
{
	Serial.begin(115200);
	// Serial1.begin(115200); // sms舵机波特率115200
	Serial2.begin(1000000); // sts舵机波特率1000000
	sms_sts.pSerial = &Serial2;
	delay(1000);
}

void loop()
{
	uint8_t ID[] = {1, 2};
	uint8_t rxPacket[4];
	int16_t Position;
	int16_t Speed;

	sms_sts.syncReadPacketTx(ID, sizeof(ID), SMS_STS_PRESENT_POSITION_L, sizeof(rxPacket)); //同步读指令包发送

	uint8_t i;
	for (i = 0; i < sizeof(ID); i++)
	{
		//接收ID[i]同步读返回包
		if (!sms_sts.syncReadPacketRx(ID[i], rxPacket))
		{
			Serial.print("ID:");
			Serial.println(ID[i]);
			Serial.println("sync read error!");
			continue; //接收解码失败
		}
		Position = sms_sts.syncReadRxPacketToWrod(15); //解码两个字节 bit15为方向位,参数=0表示无方向位
		Speed = sms_sts.syncReadRxPacketToWrod(15);	   //解码两个字节 bit15为方向位,参数=0表示无方向位
		Serial.printf("ID:%d Position:%d Speed:%d\r\n", ID[i], Position, Speed);
		// Serial.print("ID:");
		// Serial.println(ID[i]);
		// Serial.print("Position:");
		// Serial.println(Position);
		// Serial.print("Speed:");
		// Serial.println(Speed);
	}
	delay(10);
}
